import numpy
import re
import datetime
import time
import pandas as pd
import json
import os

import numpy as np

# // Obstacle type
ObstacleType={
    0:'Invalid',
    1:'Vehicle',
    2:'Pedestrian',
    3:'Rider',
    4:'Traffic_cone',
    5:'Animal',
    6:'Road_debris',
    7:'Fence',
    'Vehicle':{
        0:'unknown',
        1:'bus',
        2:'car',
        3:'truck',
        4:'special',
        5:'tiny',
        6:'van'
    },
    'Rider':{
        0: 'unknown',
        1: 'cyclist',
        2: 'motorcyclist',
        3: 'tricyclist'
    },
    'Pedestrian':{
        0:'unknown',
        1:'adult',
        2:'child'
    }
}

def get_type(type_data):
    name=[]
    for type in type_data:
        if type[0]>=1 and type[0]<=3:
            name.append(ObstacleType[ObstacleType[type[0]]][type[1]])
        else:
            name.append('other')
    return name

path='/home/king/workplace/data/data28_2021_04_03'
# path='/home/king/workplace/data/data28_2021_04_03/'
image_path=path+'/pr_label_json/'
log_file=path+'/lidar_data28.log'
# head=['id','frame_id','nearest_point_rel_x','nearest_point_rel_y','center_x','center_y',
#               'center_z','velocity_rel_x','velocity_rel_y','velocity_rel_abs','velocity_rel_sd_x',
#               'velocity_rel_sd_y','acceleration_rel_x','acceleration_rel_y','acceleration_rel_abs',
#               'acceleration_rel_sd_x','acceleration_rel_sd_y','heading_rel','length','width','height',
#               'confidence','temp_obstacle_ptr_type','temp_obstacle_ptr_vehicle_subtype',
#               'temp_obstacle_ptr_rider_subtype','ego_velocity','ego_acceleration']

head = ['id', 'frame_id', 'nearest_point_rel_x','nearest_point_rel_y','center_x','center_y','center_z',
        'velocity_rel_x','velocity_rel_y','velocity_rel_sd_x','velocity_rel_sd_y',
        'acceleration_rel_x','acceleration_rel_y','acceleration_rel_sd_x','acceleration_rel_sd_y',
        'heading_rel', 'heading_rel_sd', 'length', 'width', 'height','confidence',
        'temp_obstacle_ptr_type','temp_obstacle_ptr_vehicle_subtype','temp_obstacle_ptr_rider_subtype',
        'ego_velocity','ego_acceleration',
        'bounding_box_points_1x','bounding_box_points_1y','bounding_box_points_2x','bounding_box_points_2y',
        'bounding_box_points_3x','bounding_box_points_3y','bounding_box_points_4x','bounding_box_points_4y']

# log print for analysis
#     lidar_object_output
#         +=std::to_string(temp_obstacle_ptr->id) + ","
#         + std::to_string(lidar_packet_ptr_->GetPacket()->frame_id) + ","
#         + std::to_string(temp_obstacle_ptr->nearest_point_rel.x) + ","
#         + std::to_string(temp_obstacle_ptr->nearest_point_rel.y) + ","
#         + std::to_string(temp_object.center.x()) + ","
#         + std::to_string(temp_object.center.y()) + ","
#         + std::to_string(temp_obstacle_ptr->velocity_rel.x) + ","
#         + std::to_string(temp_obstacle_ptr->velocity_rel.y) + ","
#         + std::to_string(temp_obstacle_ptr->velocity_rel_sd.x) + ","
#         + std::to_string(temp_obstacle_ptr->velocity_rel_sd.y) + ","
#         + std::to_string(temp_obstacle_ptr->acceleration_rel.x) + ","
#         + std::to_string(temp_obstacle_ptr->acceleration_rel.y) + ","
#         + std::to_string(temp_obstacle_ptr->acceleration_rel_sd.x) + ","
#         + std::to_string(temp_obstacle_ptr->acceleration_rel_sd.y) + ","
#         + std::to_string(temp_obstacle_ptr->heading_rel) + ","
#         + std::to_string(temp_obstacle_ptr->heading_rel_sd) + ","
#         + std::to_string(temp_obstacle_ptr->length) + ","
#         + std::to_string(temp_obstacle_ptr->width) + ","
#         + std::to_string(temp_obstacle_ptr->height) + ","
#         + std::to_string(temp_obstacle_ptr->confidence) + ","
#         + std::to_string(static_cast<std::int16_t>(temp_obstacle_ptr->type)) + ","
#         + std::to_string(static_cast<std::int16_t>(temp_obstacle_ptr->vehicle_subtype)) + ","
#         + std::to_string(static_cast<std::int16_t>(temp_obstacle_ptr->rider_subtype)) + ","
#         + std::to_string(ego_velocity) + ","
#         + std::to_string(ego_acceleration) + ",";
#     for (std::uint8_t i{0}; i < 4; ++i) {
#       lidar_object_output
#         +=std::to_string(temp_obstacle_ptr->bounding_box_points[i].x) + ","
#         + std::to_string(temp_obstacle_ptr->bounding_box_points[i].y) + ",";
#     }

with open(log_file, 'r') as f:
    data_label_gt = f.read()

searchObj = re.findall( r'Lidar Object Output:(.*?)\n', data_label_gt)
datas=[]
data_id=[]
timestamp_temp='0'
timestamp='0'
count_s=0
count_f=0
for txts in searchObj:
    data=re.findall( r'\] (.*)', txts)[0]
    data = np.array(np.mat(data))[0]
    if data.__len__():
        count_f+=1
        count_s+=1
        datas_temp=data.reshape(int(data.__len__()/34),34)
        # datas_temp=np.concatenate((np.zeros([datas_temp.__len__(),1]),datas_temp),axis=1)
        # datas.append(data_temp)
        data_id.append(data[0])
        time_local = time.localtime(int(re.findall(r'\[(.*?)\]', txts)[0]) / 1e6)
        timestamp=time.strftime("%Y-%m-%d_%H-%M-%S-", time_local)
        if timestamp_temp!=timestamp:
            count_s=0
        timestamp_temp = timestamp
        data_temp = {
            'point_cloud': {
                'frame_id': count_f,
                'next_frame': count_f+1,
                'previous_frame': count_f-1,
                'point_file': -1,
                'offset_z': 1},
            'image_file': timestamp+str(count_s)+'.jpg',
            'radar_file': timestamp+str(count_s)+'.bin',
            'muti_sensor_cfg': 0,
            'annos': {
                'name': get_type(datas_temp[:,21:23]),  # * str < ----- Check:-1 | shape: 1 # temp_obstacle_ptr_rider_subtype
                'gt_boxes_3d': np.concatenate([(datas_temp[:,4:7]-np.array([6.09969,-0.060999,2.7])).T,datas_temp[:,17:20].T,datas_temp[:,15:16].T]).T.tolist(),  # c <= 2 < ----- Check: -1 | shape: 2
                'num_lidar_pts': -1,  # < ----- Check:-1 | shape: 1x
                'track_id': datas_temp[:,0].T.astype(int).tolist(),  # < ----- Check:-1 | shape: 2
                'velocity': datas_temp[:,7:11].tolist(),  # float / -1 < ----- Check: -1 | shape: 2
                'acceleration': datas_temp[:,11:15].tolist(),  # float / -1 < ----- Check: -1 | shape: 2
                'confidence':datas_temp[:,20:21].T.tolist()[0],
                'others': 0
            }
        }
        print(get_type(datas_temp[:,22:25]))
        print(datas_temp[:,23:24].T)
        if not os.path.exists(image_path):
            os.mkdir(image_path)
        print(data_temp)
        with open(image_path + timestamp+str(count_s)+'.json', 'w') as f:
            json.dump(data_temp, f)

# datas=np.concatenate(datas)
# datas=pd.DataFrame(datas)
# datas.columns=head
# N=5


